package pathfinder

import "C"
import (
	"math"
	"src/protoMsg"
	"src/zeus/linmath"
	"src/zeus/logger"
	"unsafe"
)

/*
新写的ObsNavMesh

*/

type FinderObs struct {
	Iskey bool //是否有Key,有Key的不能修改墙信息
	//地图的引用，释放的时候，只需要释放指针就好
	NavMesh *Cgo_ObsNavMesh
	// 寻路用对象，每个地图实例一个，释放的时候，需要delete
	Finder *Cgo_ObsPathFinder
}

func (this *FinderObs) Destroy() {
	if this.NavMesh != nil {
		this.NavMesh = nil
		if !this.Iskey {
			logger.Debugf(logger.LogKey_Lib, "FinderObs.Destroy(%p)", this.NavMesh)
		}
	}
	if this.Finder != nil {
		this.Finder.FreeObsPath()
		this.Finder = nil

	}
}

// FindPath 提供外部寻路算法
//
// 参数：
//
//	params 起点终点
//	findtype 切换寻路类型
//
// 返回值：
//
//	path 路点，注意第一个点就是输入的起始点
//	result 是否寻路成功
func (this *FinderObs) FindPath(stpos, edpos linmath.Vector3) (path []linmath.Vector3, result bool) {

	spos := this.serverToNav(&stpos)
	epos := this.serverToNav(&edpos)

	count := int32(0)
	var floats *C.float
	if floats = this.Finder.FindNavPath(spos, epos, &count); count == 0 {
		return nil, false
	}

	path = make([]linmath.Vector3, count, count)
	retPtr := unsafe.Pointer((*C.float)(floats))
	size := unsafe.Sizeof(linmath.Vector3{})
	for i := 0; i < int(count); i++ {
		linVec := (*linmath.Vector3)(unsafe.Pointer(uintptr(retPtr) + uintptr(i)*size))
		path[i] = *this.navToServer(linVec)
	}
	logger.Debug(logger.LogKey_MoveSys, "[FindNavmeshCpp] FindPath 寻路成功 路点个数 ", count)
	return path, true
}

// GetNear 获取某个点附近的合法位置
// 参数：
//
//	params 检查点 *linmath.Vector3 类型
//
// 返回值：
//
//	pos 路点，注意第一个点就是输入的起始点
//	result 是否能成功
func (this *FinderObs) GetNear(srcPos *linmath.Vector3) (pos *linmath.Vector3, result bool) {
	if srcPos == nil {
		logger.Error("GetNear params not match")
		return nil, false
	}
	spos := this.serverToNav(srcPos)
	epos := linmath.NewVector3(0, 0, 0)

	var floats *C.float
	var count int32
	if floats = this.Finder.CalcNavNear(spos, &count); count == 0 {
		return nil, false
	}
	retPtr := unsafe.Pointer((*C.float)(floats))
	size := unsafe.Sizeof(linmath.Vector3{})
	for i := 0; i < int(count); i++ {
		linVec := (*linmath.Vector3)(unsafe.Pointer(uintptr(retPtr) + uintptr(i)*size))
		epos = *this.navToServer(linVec)
	}
	return &epos, true
}

// CheckWall 按照100单位步长检查墙
// 墙的定义点位高程超过500单位
func (this *FinderObs) CheckWall(srcPos, dstPos linmath.Vector3) (path []linmath.Vector3, bHaveWall bool) {
	bHaveWall = false

	srcPos.Y = dstPos.Y
	logger.Debug(logger.LogKey_MoveSys, "CheckWall srcPos ", srcPos, " dstPos ", dstPos)

	dir := dstPos.Sub(srcPos)
	dst := float64(dir.Len())
	dir.Normalize()

	minDst := 100.0

	stepCount := int(math.Ceil(dst / minDst))

	tmpPos := *linmath.Vector3_Zero()
	for i := 0; i < stepCount; i++ {
		curDst := float32(float64(i+1) * minDst)
		tmpPos = srcPos.Add(dir.Mul(curDst))
		if nearPos, ok := this.GetNear(&tmpPos); ok {
			if !nearPos.IsEqualXZ(tmpPos) {
				path = append(path, *nearPos)
				logger.Debug(logger.LogKey_MoveSys, "nearPos ", *nearPos)
				bHaveWall = true
				return
			}
		} else {
			logger.Debug(logger.LogKey_MoveSys, "nearPos not find near tmpPos ", tmpPos)
			bHaveWall = true
			return
		}
	}
	return
}

// Raycast 射线检测，是否撞到墙了
// 参数：
//
//	srcPos 起点
//	dstPos 终点
//
// 返回值：
//
//	pos 如果撞到墙了将会返回墙体位置，否则为空
//	result 是否碰到墙了，true 碰到墙 false 没有撞到墙
func (this *FinderObs) RaycastEx(srcPos, dstPos linmath.Vector3) (*linmath.Vector3, bool) {

	spos := this.serverToNav(&srcPos)
	epos := this.serverToNav(&dstPos)

	var floats *C.float
	var count int32
	if floats = this.Finder.CalcRaycast(spos, epos, &count); count == 0 {
		return nil, false
	}
	path := make([]linmath.Vector3, count, count)
	retPtr := unsafe.Pointer((*C.float)(floats))
	size := unsafe.Sizeof(linmath.Vector3{})
	for i := 0; i < int(count); i++ {
		linVec := (*linmath.Vector3)(unsafe.Pointer(uintptr(retPtr) + uintptr(i)*size))
		path[i] = *this.navToServer(linVec)
	}
	return &(path[0]), true
}

// 动态阻挡相关
// 增加可旋转的Box阻挡
func (this *FinderObs) CreateObstacleOBBBox(doorName string, centPos, halfPos linmath.Vector3, YawDegree int32) {
	if this.Iskey {
		return
	}
	angle := linmath.AngleTo180(float32(YawDegree))
	yawRedian := linmath.AngleToRadian(angle)
	this.NavMesh.ObsAddObbBox(doorName, &centPos, &halfPos, float32(yawRedian))
	this.NavMesh.ObsUpdateTO()
}

// 增加圆柱体阻挡
func (this *FinderObs) CreateObstacleCylinder(doorName string, centPos linmath.Vector3, radius, height int32) {
	if this.Iskey {
		return
	}
	centPos = linmath.PosClient2Nav(&centPos)
	fradius := linmath.Client2NavScale(float32(radius))
	fheight := linmath.Client2NavScale(float32(height))
	this.NavMesh.ObsAddCylinder(doorName, &centPos, fradius, fheight)
	this.NavMesh.ObsUpdateTO()
}

// 清理掉某个门
func (this *FinderObs) RemoveObstacle(doorName string) {
	if this.Iskey {
		return
	}
	this.NavMesh.ObsRemoveTOByTag(doorName)
	this.NavMesh.ObsUpdateTO()
}

// 清理全部的门
func (this *FinderObs) RemoveAllObstacle() {
	if this.Iskey {
		return
	}
	this.NavMesh.ObsRemoveAllTO()
}

// 更新阻挡信息
func (this *FinderObs) UpdateObstacle() {
	if this.Iskey {
		return
	}
	this.NavMesh.ObsUpdateTO()
}

func (this *FinderObs) MeshCheckRushPath(srcPos, stpos linmath.Vector3) linmath.Vector3 {

	if tmpPos, ok := this.RaycastEx(srcPos, stpos); !ok {
		logger.Debug(logger.LogKey_MoveSys, " [FindCollection] MeshCheckRushPath 未能检测到墙直接通过 ", stpos)
		return stpos
	} else {
		/*
			这是为了定在墙根的时候，可以尝试挪位置
			但是这个逻辑其实写的问题，所以先去掉。
		*/
		// if tmpPos.IsEqualXZ(srcPos) {
		// 	if path, ok := this.FindPath(srcPos, stpos); ok {
		// 		if len(path) == 2 {
		// 			*tmpPos = path[1]
		// 			logger.Debug(logger.LogKey_MoveSys, " [FindCollection] MeshCheckRushPath 检测到墙 再次尝试走到墙根 ", path[1])
		// 		}
		// 	}
		// } else {
		logger.Debug(logger.LogKey_MoveSys, " [FindCollection] MeshCheckRushPath 检测到墙 ", tmpPos)
		// }
		stpos = *tmpPos
		return stpos
	}
}

//-----------------------------------------------------------------------

/*
这两个函数是用来做单位装换的。
服务器是米为单位，然后根据客户端的单位做转换逻辑
*/

func (this *FinderObs) serverToNav(vec3 *linmath.Vector3) *linmath.Vector3 {
	return &linmath.Vector3{
		Vector3: protoMsg.Vector3{X: (vec3.X) * 1, Y: (vec3.Y) * 1, Z: (vec3.Z) * 1},
	}
}

func (this *FinderObs) navToServer(vec3 *linmath.Vector3) *linmath.Vector3 {
	return &linmath.Vector3{
		Vector3: protoMsg.Vector3{X: vec3.X * 1, Y: vec3.Y * 1, Z: vec3.Z * 1},
	}
}
